/**
 * Visualization tool for Hokuyo URG-04LX Laser Sensor.
 * Copyright 2007 - Brian Ziman [bziman(at)acm.org]
 * http://www.brianziman.com/
 */
package fr.imag.m2pgi.m2m.geoloc.gui;

import java.awt.*;
import javax.swing.*;
import javax.swing.event.*;

import fr.imag.m2pgi.m2m.geoloc.Lidar;

import java.awt.event.*;
import java.io.IOException;

import java.util.*;

/*  ^ 8192
 *  |        0
 *  | 90     *     -90
 *  |  120       -120
 *  v <----8192------->
 */
public class LaserVisualizer extends JPanel implements Runnable {

  /**
   * 
   */
  private static final long serialVersionUID = -3005174212062385900L;
  
  double xmin = -4096;
  double xmax = 4096;
  double ymin = -4096;
  double ymax = 4096;

  private Lidar gLaser;
  private boolean stop;

  private static boolean useLog = false;

  public LaserVisualizer(int width, int height) {
    setDoubleBuffered(true);
    setSize(width, height);
  }
  
  public void setLaser(Lidar lidar) {
    System.out.println("Salut je rentre");
    this.gLaser = lidar;
    System.out.println("Wahou j'ai initialisé le laser!");
    try {
      if (!gLaser.isEnabled()) {
        System.out.println("Ah c'est pas activé!!");
        gLaser.enable();
        System.out.println("Maintenant, oui.");
      } else {
        System.out.println("Laser activé! Faites sauter Alderaan!");        
      }
    } catch (IOException e1) {
      // TODO Auto-generated catch block
      e1.printStackTrace();
    }
  }

  Font font = new Font("FreeMono", Font.BOLD, 12);

  double maxrange = 0;

  public void paint(Graphics g) {
    try {
      ArrayList<Integer> data = gLaser.getMeasures();
      g.setColor(Color.BLACK);
      g.fillRect(0, 0, getWidth(), getHeight());

      g.setFont(font);
      g.setColor(Color.WHITE);
      try {
        g.drawString("Range(0): " + gLaser.getRange(data, 0), 20, 70);
      } catch (IOException e1) {
        // TODO Auto-generated catch block
        e1.printStackTrace();
      }

      // square!
      double scale = Math.min(getWidth(), getHeight())
          / (useLog ? 2 * Math.log(5000d) : 10000d);

      int x0 = getWidth() / 2;
      int y0 = getHeight() / 2;

      for (double i = -120; i <= 120; i++) {
        double range = useLog ? Math.log(4000) : 4000;
        int x1 = (int) (x0 - scale * range * Math.sin(Math.toRadians(i)));
        int y1 = (int) (y0 - scale * range * Math.cos(Math.toRadians(i)));
        g.setColor(Color.BLUE);
        g.drawLine(x0, y0, x1, y1);

        try {
          range = gLaser.getRange(data, i);
        } catch (IOException e) {
          // TODO Auto-generated catch block
          e.printStackTrace();
        }
        if (range > maxrange)
          maxrange = range;
        if (range == 0) {
          range = maxrange;
          g.setColor(Color.YELLOW); // yellow = maybe = prob. under est.
        } else if (range < 20) {
          range = maxrange;
          g.setColor(Color.RED); // red = stop = just guessing
          // System.out.println(gLaser.getErrorFromData(range));
        } else {
          g.setColor(Color.GREEN); // green = go = good data
        }
        range = useLog ? Math.log(range) : range;
        x1 = (int) (x0 - scale * range * Math.sin(Math.toRadians(i)));
        y1 = (int) (y0 - scale * range * Math.cos(Math.toRadians(i)));
        g.drawLine(x0, y0, x1, y1);
      }
    } catch (IOException e1) {
      // TODO Auto-generated catch block
      e1.printStackTrace();
    }
  }

  synchronized boolean isStop() {
    return stop;
  }

  synchronized void setStop(boolean stop) {
    this.stop = stop;
  }

  public void run() {
    setVisible(true);
    while (!isStop()) {
      repaint();
      try {
        Thread.sleep(200);
      } catch (InterruptedException e) {
        System.out.println("Error: interrupted!");
        return;
      }
    }

  }

}
